#include "serial.hpp"
#include <demo_board/ButtonState.h>
#include <demo_board/LedCommand.h>
#include <ros/ros.h>

int main(int argc, char *argv[]) {
	ros::init(argc, argv, "demo_board_node");
	ros::NodeHandle nh;

	std::string device_path = "/dev/ttyUSB0";
	ros::param::get("~device_path", device_path);
	DemoBoard board(device_path);

	ros::Subscriber cmd_led_sub = nh.subscribe<demo_board::LedCommand>(
	    "cmd_led", 1, [&](const demo_board::LedCommand::ConstPtr &msg) {
		    outgoing_msg msg_out;
		    msg_out.led_on = msg->led_on;
		    board.send_message(msg_out);
	    });

	ros::Publisher button_state_pub =
	    nh.advertise<demo_board::ButtonState>("button_state", 1, true);

	ros::Rate rate(20);
	while (ros::ok()) {

		auto msg_in = board.poll_message();
		if (msg_in.has_value()) {
			demo_board::ButtonState ros_msg;
			ros_msg.button_pressed = msg_in->button_pressed;
			button_state_pub.publish(ros_msg);
		}

        ros::spinOnce();
		rate.sleep();
	}
}
